/**
 * This class contains the major sever-client commands interpretation and execution code
 */

package com.mcp_31091.roomba.Views;

import static com.mcp_31091.roomba.RoombaActivity.btThread;
import static com.mcp_31091.roomba.connection.ConnectedThread.MSG_MOVE_SAMPLE;

import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.OutputStreamWriter;
import java.io.PrintWriter;
import java.net.ServerSocket;
import java.net.Socket;
import java.util.List;

import android.app.Activity;
import android.content.Context;
import android.net.wifi.ScanResult;
import android.net.wifi.WifiManager;
import android.os.Bundle;
import android.os.Message;
import android.util.Log;

import com.mcp_31091.R;
import com.mcp_31091.roomba.RoutePlan.RoutePlanner;
import com.mcp_31091.roomba.WifiSignalMap.MapGenerator;
import com.mcp_31091.roomba.helper.Helper;
import com.mcp_31091.roomba.localizer.Localizer;

public class RoombaCommandsView extends Activity {
	public static final int REQUEST_ENABLE_BT = 0;
	public static final String TAG = "DebugInfo";
	protected static final char CMD_MOVE_FORWARD = 0x01;
	protected static final char CMD_MOVE_BACKWARD = 0x02;
	protected static final char CMD_SPIN_LEFT = 0x03;
	protected static final char CMD_SPIN_RIGHT = 0x04;
	protected static final char CMD_SEND_WIFI = 0x05;
	private int yPos = 0, xPos = 0; // because the x and y coordinates are in
	private double orientation;
	private WifiManager wifi;
	@Override
	protected void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		setContentView(R.layout.test_drive_panel);
		setupViews();
		setupServer();
		wifi = (WifiManager) getSystemService(Context.WIFI_SERVICE);
	}

	private void setSpeed(int speed) {
		btThread.speed = speed;
	}

	int preX = 0;
	int preY = 0;
	public void spinCalculation(double degree) {
		float pausetime = 100 / 1000 + (float) Math
				.abs(((float) (230 * Math.PI / 360.0)) * 103.8 / 100);
		int realpause = (int) (pausetime * 1000 * degree / 90);
		btThread.drive(100, 1);
		btThread.pause(realpause);
		btThread.stopDrive();
	}
	
	public void move(double turn, int forward) {
		if (forward == 0)
			return;
		if (turn != 0)
			spinCalculation(turn);

		orientation += turn;// + new Random().nextGaussian()*turn_noise;
		orientation %= 360;
		int distance = forward; // + new Random().nextGaussian()*forward_noise;
		xPos += (int) (distance * Math.cos(orientation * 2 / 360 * Math.PI));
		yPos += (int) (distance * Math.sin(orientation * 2 / 360 * Math.PI));
	}

	void setupViews() {
		setSpeed(150); // a little fast
	}

	void reset() {
		orientation = 0;
		xPos = 0;
		yPos = 0;
	}

	void spinLeft() {
		// btThread.spinLeft(90);
		btThread.newTurn(85);
		orientation += 90;// + new Random().nextGaussian()*turn_noise;
		orientation %= 360;
	}

	void spinRight() {
		// btThread.spinRight(90);
		btThread.newTurn(-85);
		orientation += 360;
		orientation -= 90;
		orientation %= 360;
	}

	/*Sense and send wifi signal*/
	private void sendWifiSignal(PrintWriter out3) {
		String signal = "";
		wifi.startScan();
		List<ScanResult> scanResults = wifi.getScanResults();
		signal += "x:," + xPos + ",y:," + yPos + ",";
		for (ScanResult sr : scanResults) {
			Log.i("SignalTag", sr.toString());
			signal += sr.BSSID + "," + sr.level + ",";
		}
		out3.println(signal); // send one point of signals at once
	}

	/*Setup a server to communicate with the processing client program*/
	private void setupServer() {
		try {
			new Thread(new Runnable() {

				BufferedReader bufferedReader;
				ServerSocket serverSocket = new ServerSocket(12345);
				Socket clientSocket = null;
				PrintWriter out;

				public void run() {
					while (true) {
						try {
							clientSocket = serverSocket.accept();
							// Create a reader
							try {
								bufferedReader = new BufferedReader(
										new InputStreamReader(clientSocket
												.getInputStream()));
								out = new PrintWriter(new BufferedWriter(
										new OutputStreamWriter(clientSocket
												.getOutputStream())), true);

								out.println("client ip is "
										+ clientSocket.getLocalAddress());
								// int cmd = bufferedReader. read(); //read one
								// byte
								String s;
								while ((s = bufferedReader.readLine()) != null) {

									String[] temp = s.split(",");
									Log.e("LINE",s);
									if (temp.length > 0) {
										if (temp[0].equals("forward")) {  //Foward commmand

											btThread.newDrive(Integer
													.parseInt(temp[1]));
											move(0, Integer.parseInt(temp[1]));

										} else if (temp[0].equals("backward")) { //Backward command

											btThread.newDrive(-1
													* Integer.parseInt(temp[1]));
											move(0,
													-1
															* Integer
																	.parseInt(temp[1]));
										} else if (temp[0].equals("spinLeft")) {  //Spinleft command
											spinLeft();
										} else if (temp[0].equals("spinRight")) {  //SpinRight command

											spinRight();
										} else if (temp[0].equals("setSpeed")) {   //SetSpeed command
											out.println("SPEED set to:"
													+ temp[1]);
											setSpeed(Integer.parseInt(temp[1]));
										} else if (temp[0].equals("sendWifi")) {   //SendWifi command
											sendWifiSignal(out);
											// out.println("Wifi Scan Complete!!!!");
										} else if (temp[0].equals("clear")) {   //Reset xpos, ypos, orientation command
											xPos = 0;
											yPos = 0;
											orientation = 0;
										} else if (temp[0].equals("set")   // Set xpos, ypos, orientation command
												&& temp.length == 4) {
											xPos = Integer.parseInt(temp[1]);
											yPos = Integer.parseInt(temp[2]);
											orientation = Integer
													.parseInt(temp[3]);
										}else if (temp[0].equals("parseMap"))  //Parse map file command
										{
											out.println("parse map back");
											MapGenerator.getInstance().generateMap();
											//parseMapFileTest();
											
										}else if (temp[0].equals("generateParticles"))  //Generate particles command
										{
											Localizer.getInstance().generateParticles();
										}else if(temp[0].equals("localize"))	//Localize command
										{
											out.println("LOCALIZE " + temp[1]);
											Localizer.getInstance().localize(Integer.parseInt(temp[1]));
										}else if(temp[0].equals("setMap")){	   //Navigation command
											int intx=0,inty=0,destx=0,desty=0;
											if (Integer.parseInt(temp[1])<0)
												intx=Integer.parseInt(temp[1])*-1/1000-1;
											else if (Integer.parseInt(temp[1])==0)
											{
												intx = 0;
											}else{
												intx=Integer.parseInt(temp[1])/1000-1;
											}
											
											if (Integer.parseInt(temp[2])<0){
												inty=Integer.parseInt(temp[2])*-1/1000-1;
											}
											else if (Integer.parseInt(temp[2])==0)
											{
												inty = 0;
											}
											else
											{
												inty=Integer.parseInt(temp[2])/1000-1;
											}
											
											if (Integer.parseInt(temp[3])<0){
												destx=Integer.parseInt(temp[3])*-1/1000-1;
											}
											else if (Integer.parseInt(temp[3])==0)
											{
												destx = 0;
											}
											else
											{
												destx=Integer.parseInt(temp[3])/1000-1;
											}
											
											if (Integer.parseInt(temp[4])<0){
												desty=Integer.parseInt(temp[4])*-1/1000-1;
											}
											else if (Integer.parseInt(temp[4])==0)
											{
												desty = 0;
											}
											else
											{
												desty=Integer.parseInt(temp[4])/1000-1;
											}
											out.println("POINT"+intx+inty+destx+desty);
											
											RoutePlanner.getInstance().setInit(inty,intx);
											RoutePlanner.getInstance().setDest(desty,destx);
											autoDrive();
										}else if(temp[0].equals("sample"))  //Sample command
										{
											System.out.println("GetSampleCMD");
											System.out.println("sample " + temp[1]);
											out.println("SAMPLE " + temp[1]);
											sample(out, Integer.parseInt(temp[1]));
										}
									}
								}
								
							} catch (IOException e) {
								e.printStackTrace();
							}

						} catch (IOException e) {
							System.out.println(e);
						}

						finally {
							if (clientSocket != null) {
								try {
									clientSocket.close();
								} catch (IOException e) {
									e.printStackTrace();
								}
							}
							if (bufferedReader != null) {
								try {
									bufferedReader.close();
								} catch (IOException e) {
									e.printStackTrace();
								}
							}
							if (out != null) {
								out.close();
							}
						}
					}
				}

			}).start();
		} catch (IOException e) {
			e.printStackTrace();
		}
	}
	
	/*Sample to generate the wifi*/
	protected void sample(final PrintWriter out, final int distance) {
		//move foward 1 m
		//stop and send wifi signal
		new Thread(new Runnable() { 
            public void run(){
            	try {
            		while(xPos < 21000)
            		{
            			double startTime = System.currentTimeMillis();
            			Message msg = new Message();
            			msg.what = MSG_MOVE_SAMPLE;
            			msg.arg1 = distance;
            			btThread.bHandler.sendMessage(msg);
            			move(0, distance);
						Thread.sleep(8*distance);
						double endTime  = System.currentTimeMillis();
						System.out.println("Time elapsed is " + (endTime-startTime));
						//System.out.println("OUT of thread!");
						sendWifiSignal(out);
            		}
            		Helper.sendPacket("Sample finished!");
            		
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
            }
    }).start();
	}

	/*Navigation*/
	private void autoDrive() {
		RoutePlanner.getInstance().search();
		String directive = RoutePlanner.getInstance().driveInstruction();
		System.out.println("DIR"+directive);
		String[] temp = directive.split(",");
		if (temp.length > 0) {
			for (int i = 0; i < temp.length; i++) {
				//btThread.bHandler.sendEmptyMessage(MSG_DELAY);
				if (temp[i].equals("forward")) {
					btThread.newDrive(1000);
					move(0, 1000);
				} else if (temp[i].equals("backward")) {
					btThread.newDrive(-1 * 1000);
					move(0, -1 * 1000);
					if (orientation ==90)
					{
						spinRight();
						btThread.newDrive(1000);
//						btThread.bHandler.sendEmptyMessage(MSG_DELAY);
						move(0, 1000);
					} else if (orientation ==270){
						spinLeft();
						btThread.newDrive(1000);
//						btThread.bHandler.sendEmptyMessage(MSG_DELAY);
						move(0, 1000);
					} else if (orientation == 180){
						btThread.newDrive(1000);
//						btThread.bHandler.sendEmptyMessage(MSG_DELAY);
						move(0, 1000);
					} else if(orientation == 0){
						
					}
				} else if (temp[i].equals("left")) {
					spinLeft();
//					btThread.bHandler.sendEmptyMessage(MSG_DELAY);
				} else if (temp[i].equals("right")) {
					spinRight();
//					btThread.bHandler.sendEmptyMessage(MSG_DELAY);
				}
			}
		} else {
			System.out.println("NO");
		}
	}
}
